#include <fstream>
#include <iostream>
#include <TCanvas.h>
#include <TGraph.h>
#include <TH1F.h>
#include <TMath.h>
#include <TVector3.h>

void ge_calib_pos_hist(TCanvas *, int);
double doppler_correct(double, double, double, double);
double find_error(double, double, double [3][6], double [24]);
void turn(double, double, double, double, double *, double *);

void ge_calib_pos_hist(TCanvas *canvas, int in_clu)
{
  std::ifstream file("hist_precond.txt");
  double peak_list[8][3][6];
  for (int clu = 0; 8 > clu; ++clu) {
    for (int cry = 0; 3 > cry; ++cry) {
      for (int seg = 0; 6 > seg; ++seg) {
        file >> peak_list[clu][cry][seg];
      }
    }
  }

  double start_angles[8][4] = {
    {119.90,   0.00, 288.17, 184.17},
    { 67.33,   0.00, 206.69, 171.25},
    { 66.45,   0.00,  90.03, 178.01},
    {120.68,   0.00, 186.97, 214.69},
    {123.50,   0.00,  93.70, 165.16},
    { 64.99,   0.00,  85.03, 173.64},
    {117.50,   0.00, 322.27, 182.79},
    { 63.96,   0.00,  87.66, 150.89}
  };

  double const kRange = 1;

  double energy_beam = 2.85;
  double gamma_beam = energy_beam / 941. + 1.;
  double beta_beam = sqrt(1. - 1. / (gamma_beam * gamma_beam));

//  canvas->SetOptStat(0);

  double th[24], ph[24];
  int const N = 1001;
  double x[N], y[N];
  double min;
  double mini;

  std::cout << '\n';

  canvas->cd(1);
  min = 1e9;
  mini = -1.0;
  for (int i = 0; i < N; ++i) {
    double theta = i * kRange * 180.0 / (N - 1.) +
        start_angles[in_clu][0] - 0.5 * kRange * 180.0;
    turn(theta, start_angles[in_clu][1], start_angles[in_clu][2],
        start_angles[in_clu][3], th, ph);
    x[i] = theta;
    y[i] = find_error(beta_beam, gamma_beam, peak_list[in_clu], th);
    if (min > y[i]) {
      min = y[i];
      mini = theta;
    }
  }
  TGraph *g1 = new TGraph(N, x, y);
  g1->Draw("ALP*");
  std::cout << "Theta: " << mini << '\n';

  canvas->cd(2);
  min = 1e9;
  mini = -1.0;
  for (int i = 0; i < N; ++i) {
    double phi = i * 360. / (N - 1.);
    turn(start_angles[in_clu][0], phi, start_angles[in_clu][2],
        start_angles[in_clu][3], th, ph);
    x[i] = phi;
    y[i] = find_error(beta_beam, gamma_beam, peak_list[in_clu], th);
    if (min > y[i]) {
      min = y[i];
      mini = phi;
    }
  }
  g1 = new TGraph(N, x, y);
  g1->Draw("ALP*");
  std::cout << "Phi: " << mini << '\n';

  canvas->cd(3);
  min = 1e9;
  mini = -1.0;
  for (int i = 0; i < N; ++i) {
    double alpha = i * kRange * 360. / (N - 1.) +
        start_angles[in_clu][2] - 0.5 * kRange * 360.0;
    turn(start_angles[in_clu][0], start_angles[in_clu][1], alpha,
        start_angles[in_clu][3], th, ph);
    x[i] = alpha;
    y[i] = find_error(beta_beam, gamma_beam, peak_list[in_clu], th);
    if (min > y[i]) {
      min = y[i];
      mini = alpha;
    }
  }
  g1 = new TGraph(N, x, y);
  g1->Draw("ALP*");
  std::cout << "Alpha: " << mini << '\n';

  canvas->cd(4);
  min = 1e9;
  mini = -1.0;
  for (int i = 0; i < N; ++i) {
    double dist = i * kRange * 500. / (N - 1.) +
        start_angles[in_clu][3] - 0.5 * kRange * 500.0;
    turn(start_angles[in_clu][0], start_angles[in_clu][1],
        start_angles[in_clu][2], dist, th, ph);
    x[i] = dist;
    y[i] = find_error(beta_beam, gamma_beam, peak_list[in_clu], th);
    if (min > y[i]) {
      min = y[i];
      mini = dist;
    }
  }
  g1 = new TGraph(N, x, y);
  g1->Draw("ALP*");
  std::cout << "Distance: " << mini << '\n';

  std::cout << '\n';
}

double doppler_correct(double energy, double beta, double gamma, double
    cos_theta)
{
  return energy * (gamma * (1 - beta * cos_theta));
}

double find_error(double beta, double gamma, double peak_list[3][6], double
    th[24])
{
  int map[] = {1, 2, 4, 5, 6, 7};
  double dE = 0.;
  for (int cry = 0; 3 > cry; ++cry)
    for (int seg = 0; 6 > seg; ++seg)
      dE += fabs(doppler_correct(peak_list[cry][seg], beta, gamma, cos(th[cry
          * 8 + map[seg]])) - 1016.95);
  return dE;
}

void turn(double t, double p, double a, double dist, double *th, double *ph) 
{
  double const kDeg2Rad = TMath::Pi() / 180.0;
  t = t * kDeg2Rad;
  p = p * kDeg2Rad;
  a = a * kDeg2Rad;
  double const kCore2Crystal = 34.0;

  double center_x[3];
  double center_y[3];
  double center_z[3];

  // Core A.
  center_x[0] = kCore2Crystal;
  center_y[0] = 0;
  center_z[0] = dist;

  // Core B.
  center_x[1] = -kCore2Crystal * sin(30 * kDeg2Rad);
  center_y[1] = -kCore2Crystal * cos(30 * kDeg2Rad);
  center_z[1] = dist;

  // Core C.
  center_x[2] = -kCore2Crystal * sin(30 * kDeg2Rad);
  center_y[2] = kCore2Crystal * cos(30 * kDeg2Rad);
  center_z[2] = dist;

  double const kR = kCore2Crystal * 0.6;
  double const kRs = kR * sin(30.0 * kDeg2Rad);
  double const kRc = kR * cos(30.0 * kDeg2Rad);

  double x[24];
  double y[24];
  double z[24];

  // Crystal A.
  x[0] = center_x[0];        // A0 (core)
  y[0] = center_y[0];
  z[0] = center_z[0];
  x[1] = center_x[0] + kRs;  // A1
  y[1] = center_y[0] - kRc;
  z[1] = center_z[0];
  x[2] = center_x[0] - kRs;  // A2
  y[2] = center_y[0] - kRc;
  z[2] = center_z[0];
  x[3] = 0.0;                // empty DGF channel
  y[3] = 0.0;
  z[3] = 0.0;
  x[4] = center_x[0] + kR;   // A3
  y[4] = center_y[0] + 0.0;
  z[4] = center_z[0];
  x[5] = center_x[0] + kRs;  // A4
  y[5] = center_y[0] + kRc;
  z[5] = center_z[0];
  x[6] = center_x[0] - kR;   // A5
  y[6] = center_y[0] + 0.0;
  z[6] = center_z[0];
  x[7] = center_x[0] - kRs;  // A6
  y[7] = center_y[0] + kRc;
  z[7] = center_z[0];

  // Crystal B.
  x[8] = center_x[1];
  y[8] = center_y[1];
  z[8] = center_z[1];
  x[9] = center_x[1] - kR;
  y[9] = center_y[1] + 0.0;
  z[9] = center_z[1];
  x[10] = center_x[1] - kRs;
  y[10] = center_y[1] + kRc;
  z[10] = center_z[1];
  x[11] = 0.0;
  y[11] = 0.0;
  z[11] = 0.0;
  x[12] = center_x[1] - kRs;
  y[12] = center_y[1] - kRc;
  z[12] = center_z[1];
  x[13] = center_x[1] + kRs;
  y[13] = center_y[1] - kRc;
  z[13] = center_z[1];
  x[14] = center_x[1] + kRs;
  y[14] = center_y[1] + kRc;
  z[14] = center_z[1];
  x[15] = center_x[1] + kR;
  y[15] = center_y[1] + 0.0;
  z[15] = center_z[1];

  // Crystal C.
  x[16] = center_x[2];
  y[16] = center_y[2];
  z[16] = center_z[2];
  x[17] = center_x[2] + kRs;
  y[17] = center_y[2] + kRc;
  z[17] = center_z[2];
  x[18] = center_x[2] + kR;
  y[18] = center_y[2] + 0.0;
  z[18] = center_z[2];
  x[19] = 0.0;
  y[19] = 0.0;
  z[19] = 0.0;
  x[20] = center_x[2] - kRs;
  y[20] = center_y[2] + kRc;
  z[20] = center_z[2];
  x[21] = center_x[2] - kR;
  y[21] = center_y[2] + 0.0;
  z[21] = center_z[2];
  x[22] = center_x[2] + kRs;
  y[22] = center_y[2] - kRc;
  z[22] = center_z[2];
  x[23] = center_x[2] - kRs;
  y[23] = center_y[2] - kRc;
  z[23] = center_z[2];

  // matrix = m_phi * m_theta * m_alpha
  double const cp = cos(p);
  double const sp = sin(p);
  double const ct = cos(t);
  double const st = sin(t);
  double const ca = cos(a);
  double const sa = sin(a);
  // Column major.
  double matrix[9] = {
    ca * cp - ct * sa * sp, cp * ct * sa + ca * sp, -sa * st,
    -cp * sa - ca * ct * sp, ca * cp * ct - sa * sp, -ca * st,
    -sp * st, cp * st, ct
  };

  // Transform!
  for (int i = 0; 24 > i; ++i) {
    double xi = matrix[0] * x[i] + matrix[3] * y[i] + matrix[6] * z[i];
    double yi = matrix[1] * x[i] + matrix[4] * y[i] + matrix[7] * z[i];
    double zi = matrix[2] * x[i] + matrix[5] * y[i] + matrix[8] * z[i];
    th[i] = atan2(sqrt(xi * xi + yi * yi), zi);
    ph[i] = atan2(-xi, yi);
  }
}
